function theSafety=findSafety(P,Q,R,sites,triangles, triangleGrid)

%find intersecting triangle with PQR
[x_idx, y_idx]=findIdx([P(1); Q(1); R(1)], [P(2); Q(2); R(2)], triangleGrid);
[BBox_x, BBox_y]=meshgrid(min(x_idx):1:max(x_idx),min(y_idx):1:max(y_idx));
theLength=size(BBox_x,1)*size(BBox_x,2);
triangleToCheck=[];
for l=1:theLength
    triangleToCheck=union(triangleToCheck, triangleGrid.grid{BBox_x(l), BBox_y(l)});
end

%find the minimum distance between the polygon and the value
theSafety=min([P(3), Q(3), R(3)]);
for t=1:length(triangleToCheck)
    theSafety=min(theSafety, safetyVal(P,Q,R,triangles(triangleToCheck(t),:), sites));
end
end

function distance=safetyVal(P,Q,R,triangle, sites)
    %find the value when the distance between the curve PQR and the plane
    %defined by the triangle is the closest
    candidate1=minPlaneDist(P,Q,R,triangle, sites);
    
    %find the intersection of 2D triangle and the curve, one side at a time
    candidate2=intersectionLine(P,Q,R, sites(triangle(1),:), sites(triangle(2),:));
    candidate3=intersectionLine(P,Q,R, sites(triangle(2),:), sites(triangle(3),:));
    candidate4=intersectionLine(P,Q,R, sites(triangle(3),:), sites(triangle(1),:));
    
    %check end points
    candidate5=thePlaneDist(P,triangle, sites);
    candidate6=thePlaneDist(Q,triangle, sites);
    
    %return the minimum value
    candidates=[candidate1, candidate2, candidate3, candidate4, candidate5, candidate6];
    if isempty(candidates)
        distance=max([P(3), Q(3), R(3)]);
    else
        distance=min(candidates);
    end
end




%minimum distance between the curve PQR and the plane defined by triangle
%(only if it exists)
function distance=minPlaneDist(P,Q,R,triangle,sites)

%find the plane equation that passes A,B,C, [x' 1]*[a;d]=0;
pointMat=[sites(triangle',:) ones(3,1)];
coeff=null(pointMat);

%make the normal vector to point positive z direction
if coeff(3)<0
    coeff=-coeff;
end
coeff=coeff./coeff(3);

%F(t)=T2_coeff*(t*t)+T1_coeff*t+T0_coeff
T2_coeff=P-2*Q+R;
T1_coeff=2*(Q-P);
T0_coeff=P;

A=T2_coeff(3)+coeff(1)*T2_coeff(1)+coeff(2)*T2_coeff(2);
B=T1_coeff(3)+coeff(1)*T1_coeff(1)+coeff(2)*T1_coeff(2);
C=T0_coeff(3)+coeff(4)+coeff(1)*T0_coeff(1)+coeff(2)*T0_coeff(2);
t=-B/(2*A);

if A>0 && (t >=0) && (t <=1)
    point=(1-t)*(1-t)*P+2*(1-t)*t*Q+t*t*R;
    if insideTriangle(sites(triangle(1),1:2)', sites(triangle(2),1:2)', sites(triangle(3),1:2)', point(1:2))
        distance=C-(B*B/4/A);
        %distance=point(3)+coeff(4)+coeff(1)*point(1)+coeff(2)*point(2);
    else
        distance=[];
    end
else
    distance=[];
end

end




%the distance in the intersection of the curve PQR and the line segment AB 
function distance=intersectionLine(P,Q,R,A,B)

T2_coeff=P-2*Q+R;
T1_coeff=2*(Q-P);
T0_coeff=P;

%y=mx+n
m=(A(2)-B(2))./(A(1)-B(1));
n=A(2)-m*A(1);

a=T2_coeff(2)-m*T2_coeff(1);
b=T1_coeff(2)-m*T1_coeff(1);
c=T0_coeff(2)-m*T0_coeff(1)-n;

determinant=b*b-4*a*c;
if determinant <0
    distance=[];
    return;
elseif determinant==0
    t=-b/(2*a);
    if t>=0 && t<=1
        distance=theLineDist(P,Q,R,t,A,B);        
    else
        distance=[];
        return;
    end
else
    t=(-b+sqrt(determinant))/(2*a);
    distance=theLineDist(P,Q,R,t,A,B);
    t=(-b-sqrt(determinant))/(2*a);
    distance=[distance, theLineDist(P,Q,R,t,A,B)];
end

end

%the distance from the point P and the plane defined by triangle
function distance=thePlaneDist(P,triangle, sites)

if insideTriangle(sites(triangle(1),1:2)', sites(triangle(2),1:2)', sites(triangle(3),1:2)', P(1:2))
    pointMat=[sites(triangle',:) ones(3,1)];
    coeff=null(pointMat);
    coeff=coeff./coeff(3);
    z_plane=-coeff(4)-coeff(1)*P(1)-coeff(2)*P(2);
    distance=P(3)-z_plane;
else
    distance=[];
end

end

%the distance from the point and the line segment AB
function distance=theLineDist(P,Q,R,t,A,B)
    if t>=0 && t<=1
        point=(1-t)*(1-t)*P+2*(1-t)*t*Q+t*t*R;
        s=(point(1)-A(1))./(B(1)-A(1));
        if s>=0 && s<=1
            z_line=(1-s)*A(3)+s*B(3);
            distance=point(3)-z_line;
        else
            distance=[];
        end
    else
        distance=[];
    end
end